
import copy
import traceback
import cv2
from cv_bridge import CvBridge, CvBridgeError

import rospy
from sensor_msgs.msg import Image
from std_msgs.msg import Bool
from acupoint_msgs.msg import AcupointImagePixel





class AcupointDisplay:
    def __init__(self):
        self.origin_image=None
        self.bridge=CvBridge()
        self.acupoint_sub = rospy.Subscriber("/acupoint/acu_filter_image_pixel", AcupointImagePixel, self.acupointCallback) 
        self.img_sub = rospy.Subscriber("/k4a/color/color", Image,self.imgCallback,queue_size=1)
        self.x_lists=[]
        self.y_lists=[]

        self.acupoint_name=None
        print("wait for acupoint information")
        while not rospy.is_shutdown() and (self.origin_image is None or self.acupoint_name is None):
            rospy.sleep(0.1)
        
        try:
            while  not rospy.is_shutdown():
                if self.acupoint_name is not None:
                    for i in range(len(self.acupoint_name)):
                        x=self.acupoint_pixels[i].x
                        y=self.acupoint_pixels[i].y
                        y_plot = int(x)
                        x_plot = int(y)
                        cv2.circle(self.origin_image, (x_plot, y_plot), 5, (0, 255, 255), thickness=-1)
                        cv2.putText(self.origin_image, self.acupoint_name[i], (x_plot, y_plot), cv2.FONT_HERSHEY_PLAIN,1, (106, 68, 20), thickness=1)
                cv2.namedWindow("camera_display", 0)
                cv2.imshow("camera_display", self.origin_image)
                cv2.waitKey(100)
        except:
            print(traceback.format_exc())

    def imgCallback(self,image_msg):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(image_msg, "bgr8")
            self.origin_image = cv_image
        except CvBridgeError as e:
            print(e)
       
    def acupointCallback(self,msg):
        self.acupoint_name=copy.deepcopy(msg).name
        self.acupoint_pixels=copy.deepcopy(msg).pixels
    

if __name__ == "__main__":
    rospy.init_node('acupoint_display_node')
    ad=AcupointDisplay()



    

    
